#ifndef ANGULAR_H
#define ANGULAR_H

// void calculate_angular_velocity(chassis_t *chassis,double R_current,double L_current);
double R_position_past = 0.0f;
double L_position_past = 0.0f;
double Rt_position_past = 0.0f;
double Lt_position_past = 0.0f;
int revolution_count = 0; // 改进变量命名
int i = 0;

/*
计算轮子速度：chassis->wheel_motor[0].vel
计算关节速度
记录关节位置
*/
void calculate_angular_velocity(chassis_t *chassis, double R_current, double L_current, double RT_current, double LT_current)
{
    // const double PI = 3.14159265358979323846;  // 明确定义π值
    double delta_R;
    double delta_L;
    double delta_Rt;
    double delta_Lt;

    chassis->joint_motor[0].pos = LT_current;
    chassis->joint_motor[1].pos = RT_current;

    // 计算位置差（改进数值稳定性）
    if (i == 0)
    {
        delta_R = 0;
        delta_L = 0;
        delta_Rt = 0;
        delta_Lt = 0;
        i = 1;
    }
    else
    {
        delta_R = R_current - R_position_past;
        delta_L = L_current - L_position_past;
        delta_Rt = RT_current - Rt_position_past;
        delta_Lt = LT_current - Lt_position_past;
    }

    // 计算角速度（使用更精确的浮点除法）
    chassis->wheel_motor[0].vel = delta_L / TIME_step_s;
    chassis->wheel_motor[1].vel = delta_R / TIME_step_s;
    chassis->joint_motor[0].vel = delta_Lt / TIME_step_s;
    chassis->joint_motor[1].vel = delta_Rt / TIME_step_s;
    // 更新历史位置（使用总角度进行存储）
    R_position_past = R_current;
    L_position_past = L_current;
    Rt_position_past = RT_current;
    Lt_position_past = LT_current;
}
void mySaturate(float *in, float min, float max)
{
    if (*in < min)
    {
        *in = min;
    }
    else if (*in > max)
    {
        *in = max;
    }
}

#endif // UTILS_H